// 8/5 7:15
/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <math.h>
#include <std_srvs/Trigger.h>
#include <std_msgs/ColorRGBA.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_datatypes.h>
#include <mavros_msgs/PositionTarget.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Float32.h>
#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/Float64MultiArray.h"
#include <std_msgs/String.h>
#include <stdlib.h>
#include <list>

ros::Subscriber state_sub;     // 飞机当前状态cur
ros::Subscriber pose_sub;      // 飞机当前位置信息
ros::Subscriber vel_sub;       // 飞机速度信息
ros::Publisher local_pose_pub; // 设置飞机目标位置（全局坐标
ros::Publisher local_vel_pub;  // 设置飞机速度（全局坐标
ros::Publisher state_pub;      // 飞机状态
ros::Publisher set_raw_pub;    // 以飞机自己为坐标系的速度控制
ros::Publisher state_message_pub;
ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;

int land_distance = -1;

bool cur_state;

bool hold_reset_flag = false;     // 悬停重置标志
bool rotation_reset_flag = false; // 旋转重置标志
bool velocity_reset_flag = false; // 速度控制重置标志

bool state_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{ // 用于通过命令启动飞机
    cur_state = !cur_state;
    // 显示请求数据
    ROS_INFO("Publish command [%s]", cur_state == true ? "Yes" : "No");
    // 设置反馈数据
    res.success = true;
    res.message = "Change drone command state!";
    return true;
}
// 全局函数：重置悬停状态
void reset_hold_state()
{
    hold_reset_flag = true;
}

// 全局函数：重置旋转状态
void reset_rotation_state()
{
    rotation_reset_flag = true;
}

mavros_msgs::State current_state; // 订阅消息：当前状态
void state_cb(const mavros_msgs::State::ConstPtr &msg)
{
    current_state = *msg;
}

geometry_msgs::PoseStamped current_pose; // 订阅消息：当前位置（世界坐标系）
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    current_pose = *msg;
}

geometry_msgs::TwistStamped current_vel; // 订阅消息：当前速度
void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    // ROS_INFO("Current velocity: %.2f m/s", msg->twist.linear.x);
    current_vel = *msg;
}

class move_map
{
private:
    double radius = 1.0; // 半径
    double speed = 0.2;
    double angular_speed = speed / radius;
    double angle = 0.0;
    double rotation_angle;
    double last_angle = 0;

    double func(double error, double A, double B) // 变速积分的子函数，主要就是对于积分的判断
    {
        double result;
        if (error >= B + A)
        {
            result = 0;
        }
        else if (error < B + A && error > B)
        {
            result = (A + B - error) / A;
        }
        else
        {
            result = 1;
        }
        return result;
    }

public:
    int fly_to_target(geometry_msgs::PoseStamped target_pose, ros::Publisher &local_pose_pub, ros::Rate &rate)
    {
        int judge = 0;
        static geometry_msgs::PoseStamped direct_target;
        static bool first_call = true;

        // 第一次调用时初始化直接目标为当前位置
        if (first_call)
        {
            direct_target = current_pose;
            first_call = false;
        }

        // 计算到最终目标点的误差
        double ex = target_pose.pose.position.x - current_pose.pose.position.x;
        double ey = target_pose.pose.position.y - current_pose.pose.position.y;
        double ez = target_pose.pose.position.z - current_pose.pose.position.z;

        // 计算总误差距离
        double error_distance = sqrt(ex * ex + ey * ey + ez * ez);

        // 设置移动速度因子（0-1之间，数值越大移动越快）
        double base_move_factor = 0.02; // 基础移动因子，可调节

        // 实现线性-对数分段函数，以0.5米为分界点优化速度
        double distance_factor;
        const double breakpoint = 0.7; // 0.5米分界点

        if (error_distance <= breakpoint)
        {
            // 距离 <= 0.5米：使用线性函数增加速度，减少接近时间
            // 线性映射：0m->2.0倍速，0.5m->1.0倍速
            distance_factor = 2.0 - (error_distance / breakpoint);
            distance_factor = std::min(2.0, std::max(1.0, distance_factor));
        }
        else
        {
            // 距离 > 0.5米：使用对数函数抑制速度，防止过冲
            // 对数映射：0.5m->1.0倍速，随距离增加逐渐减少到0.3倍速
            double log_factor = std::log(error_distance / breakpoint + 1.0);
            distance_factor = 1.0 / (1.0 + 0.7 * log_factor); // 最低降到约0.3倍速
            distance_factor = std::min(1.0, std::max(0.3, distance_factor));
        }

        // 应用距离因子到各轴移动因子
        double move_factor_x = base_move_factor * distance_factor;
        double move_factor_y = base_move_factor * distance_factor;
        double move_factor_z = base_move_factor * distance_factor;

        // 限制最小移动因子，防止停滞
        move_factor_x = std::max(0.005, move_factor_x);
        move_factor_y = std::max(0.005, move_factor_y);
        move_factor_z = std::max(0.005, move_factor_z);

        // 更新直接目标位置，使用您提供的限幅公式
        direct_target.header.stamp = ros::Time::now();
        direct_target.header.frame_id = "map";

        // 位置限幅更新
        direct_target.pose.position.x += move_factor_x * (target_pose.pose.position.x - direct_target.pose.position.x);
        direct_target.pose.position.y += move_factor_y * (target_pose.pose.position.y - direct_target.pose.position.y);
        direct_target.pose.position.z += move_factor_z * (target_pose.pose.position.z - direct_target.pose.position.z);

        // 防止超调：如果direct_target超过了目标位置，则直接设置为目标位置
        if ((target_pose.pose.position.x - current_pose.pose.position.x) *
                (target_pose.pose.position.x - direct_target.pose.position.x) <=
            0)
        {
            direct_target.pose.position.x = target_pose.pose.position.x;
        }
        if ((target_pose.pose.position.y - current_pose.pose.position.y) *
                (target_pose.pose.position.y - direct_target.pose.position.y) <=
            0)
        {
            direct_target.pose.position.y = target_pose.pose.position.y;
        }
        if ((target_pose.pose.position.z - current_pose.pose.position.z) *
                (target_pose.pose.position.z - direct_target.pose.position.z) <=
            0)
        {
            direct_target.pose.position.z = target_pose.pose.position.z;
        }

        // 设置目标姿态 - 如果目标姿态未指定，保持当前姿态
        if (target_pose.pose.orientation.w == 0.0 &&
            target_pose.pose.orientation.x == 0.0 &&
            target_pose.pose.orientation.y == 0.0 &&
            target_pose.pose.orientation.z == 0.0)
        {
            // 目标姿态未设置，保持当前姿态
            direct_target.pose.orientation = current_pose.pose.orientation;
        }
        else
        {
            // 使用指定的目标姿态
            direct_target.pose.orientation = target_pose.pose.orientation;
        }

        // 发布限幅后的位置指令
        local_pose_pub.publish(direct_target);

        // 输出调试信息 - 修复版本
        static ros::Time last_debug_time;
        static bool debug_time_initialized = false;
        if (!debug_time_initialized)
        {
            last_debug_time = ros::Time::now();
            debug_time_initialized = true;
        }

        if ((ros::Time::now() - last_debug_time).toSec() >= 2.0)
        { // 每2秒输出一次
            ROS_INFO("Final Target: [%.3f, %.3f, %.3f], Direct Target: [%.3f, %.3f, %.3f], Current: [%.3f, %.3f, %.3f]",
                     target_pose.pose.position.x, target_pose.pose.position.y, target_pose.pose.position.z,
                     direct_target.pose.position.x, direct_target.pose.position.y, direct_target.pose.position.z,
                     current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
            ROS_INFO("Error Distance: %.3f m, Distance Factor: %.3f (%.1fm breakpoint), Move Factors: x=%.3f, y=%.3f, z=%.3f",
                     error_distance, distance_factor, breakpoint, move_factor_x, move_factor_y, move_factor_z);
            last_debug_time = ros::Time::now();
        }

        // 判断是否到达目标点 - 增加时间限制
        static ros::Time target_reached_start_time;
        static bool target_reached_timing = false;
        static bool target_reached_time_initialized = false;

        if (fabs(ex) < 0.1 && fabs(ey) < 0.1 && fabs(ez) < 0.1)
        {
            if (!target_reached_timing)
            {
                // 第一次进入目标范围，开始计时
                target_reached_start_time = ros::Time::now();
                target_reached_timing = true;
                target_reached_time_initialized = true;
                ROS_INFO("Entered target zone, starting 2-second confirmation timer...");
            }
            else
            {
                // 已经在计时中，检查是否满足1.5秒条件
                double time_in_target = (ros::Time::now() - target_reached_start_time).toSec();
                if (time_in_target >= 1.5)
                {
                    judge = 1;
                    first_call = true;              // 重置状态为下次使用
                    target_reached_timing = false;  // 重置计时状态
                    debug_time_initialized = false; // 重置调试时间初始化标志
                    ROS_INFO("Target reached! Confirmed for %.2f seconds. Resetting for next waypoint.", time_in_target);
                }
                else
                {
                    ROS_INFO("In target zone for %.2f/2.0 seconds...", time_in_target);
                }
            }
        }
        else
        {
            // 离开了目标范围，重置计时
            if (target_reached_timing)
            {
                target_reached_timing = false;
                ROS_INFO("Left target zone, resetting timer. Distance: %.3f m", error_distance);
            }
        }

        return judge;
    }

    // 改进的基于位置的速度控制：防止瞬移，安全的速度控制
    void pose_velocity_ctrl(double vx, double vy, double vz, double yaw_rate, ros::Rate &rate)
    {
        // 速度放大系数和限制
        vx *= 18; //12约为16cm/s(仿真) 24约为32cm/s(仿真)
        vy *= 18; //12约为16cm/s(仿真) 24约为32cm/s(仿真)
        vz *= 18; //12约为16cm/s(仿真) 24约为32cm/s(仿真)
        
        // 速度限制
        double vel_limit_x = 12; // 限制x轴速度
        double vel_limit_y = 12; // 限制y轴速度
        double vel_limit_z = 12; // 限制z轴速度
        double yaw_limit = 0.5;  // 限制偏航角速度
        
        // 计算时间步长
        ros::Time current_time = ros::Time::now();
        double dt = rate.expectedCycleTime().toSec();
        
        // 限制速度
        vx = std::max(-vel_limit_x, std::min(vx, vel_limit_x));
        vy = std::max(-vel_limit_y, std::min(vy, vel_limit_y));
        vz = std::max(-vel_limit_z, std::min(vz, vel_limit_z));
        yaw_rate = std::max(-yaw_limit, std::min(yaw_rate, yaw_limit));
        
        // 计算新的目标位置（基于上一次的指令位置，而不是当前实际位置）
        geometry_msgs::PoseStamped command_pose;
        command_pose.header.stamp = current_time;
        command_pose.header.frame_id = "map";
        
        // 基于计算基础位置计算新位置，避免瞬移
        command_pose.pose.position.x = current_pose.pose.position.x + vx * dt;
        command_pose.pose.position.y = current_pose.pose.position.y + vy * dt;
        command_pose.pose.position.z = current_pose.pose.position.z + vz * dt;
        
        // 计算新的偏航角
        double current_yaw = tf::getYaw(current_pose.pose.orientation);
        double new_yaw = current_yaw + yaw_rate * dt;
        
        tf::Quaternion q_tf = tf::createQuaternionFromYaw(new_yaw);
        geometry_msgs::Quaternion new_orientation;
        tf::quaternionTFToMsg(q_tf, new_orientation);
        command_pose.pose.orientation = new_orientation;
        
        // 发布新的位置指令
        local_pose_pub.publish(command_pose);
        
        // 调试信息 - 显示更多细节
        ROS_INFO_THROTTLE(1.0, "Velocity ctrl: vx=%.2f vy=%.2f vz=%.2f | target=[%.2f,%.2f,%.2f] | actual=[%.2f,%.2f,%.2f]",
                         vx, vy, vz, 
                         command_pose.pose.position.x, command_pose.pose.position.y, command_pose.pose.position.z,
                         current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
    }
    
    // 重置速度控制状态（用于话题切换时）
    void reset_velocity_control()
    {
        // 设置全局重置标志来重置速度控制状态
        velocity_reset_flag = true;
        ROS_INFO("Velocity control reset flag set");
    }

    // 基于位置的悬停，稳，放心使用
    void hold(ros::Publisher &local_pose_pub)
    {
        static geometry_msgs::PoseStamped fixed_hold_pose;
        static bool hold_initialized = false;

        // 检查是否需要重置悬停状态
        extern bool hold_reset_flag;
        if (hold_reset_flag)
        {
            hold_initialized = false;
            hold_reset_flag = false;
        }

        // 第一次调用时或重置后记录悬停位置
        if (!hold_initialized)
        {
            fixed_hold_pose = current_pose;
            fixed_hold_pose.header.stamp = ros::Time::now();
            fixed_hold_pose.header.frame_id = "map";
            hold_initialized = true;
            ROS_INFO("Hold position locked at [%.3f, %.3f, %.3f]",
                     fixed_hold_pose.pose.position.x,
                     fixed_hold_pose.pose.position.y,
                     fixed_hold_pose.pose.position.z);
        }

        // 更新时间戳但保持固定位置
        fixed_hold_pose.header.stamp = ros::Time::now();

        // 发布固定的位置命令
        local_pose_pub.publish(fixed_hold_pose);
    }

    // 重置悬停状态（用于切换到新的航路点时）
    void reset_hold()
    {
        static bool *hold_initialized_ptr = nullptr;
        if (hold_initialized_ptr == nullptr)
        {
            // 获取hold函数中静态变量的指针（通过调用一次hold来初始化）
            static bool reset_flag = true;
            if (reset_flag)
            {
                reset_flag = false;
                // 这里我们通过重新设计来避免直接访问静态变量
            }
        }
        // 由于C++的限制，我们在WaypointManager中处理重置
    }

    /**
     * 简化版绕杆飞行函数
     *
     * 功能：让无人机围绕指定点进行圆形飞行
     * 参数已简化，固定了常用的飞行参数
     *
     * @param pole_x 杆的X坐标
     * @param pole_y 杆的Y坐标
     * @param radius 绕杆半径，默认2.0米
     * @param height 飞行高度，默认3.0米
     *
     * 固定参数：
     * - 起始角度：0度
     * - 结束角度：360度（完整一圈）
     * - 角速度：0.5 rad/s
     * - 方向：顺时针
     * - 位置容差：0.3米
     *
     * 使用示例：
     * int result = 帕尔佛人民(5.0, 3.0);          // 在(5,3)点绕杆，使用默认半径和高度
     * int result = performPoleSurrounding(5.0, 3.0, 3.0);     // 在(5,3)点绕杆，半径3米，默认高度
     * int result = performPoleSurrounding(5.0, 3.0, 3.0, 4.0); // 在(5,3)点绕杆，半径3米，高度4米
     *
     * @return 0=未完成，1=完成
     */
    int performPoleSurrounding(float pole_x, float pole_y, float radius = 1.0, float height = 3.0)
    {
        // 固定参数设置
        float start_angle = 0.0;    // 起始角度：0度
        float end_angle = 2 * M_PI; // 结束角度：360度（完整一圈）
        float angular_speed = 0.5;  // 角速度：0.5 rad/s
        bool clockwise = true;      // 顺时针方向
        float pose_limit = 0.1;     // 位置容差：0.1米

        static float current_angle;
        static bool initialized = false;
        static ros::Time last_time;
        static float angle_traveled = 0;     // 累计已经转过的角度
        static float total_angle_to_travel;  // 需要转过的总角度
        static float prev_raw_angle;         // 用于追踪未标准化的角度和穿越情况
        static float raw_angle;              // 原始角度（不标准化），用于准确追踪旋转
        static float current_yaw;            // 当前偏航角（用于平滑过渡）
        static bool yaw_initialized = false; // 偏航角初始化标志

        // 第一次调用时初始化
        if (!initialized)
        {
            // 根据无人机当前位置计算起始角度
            float current_x = current_pose.pose.position.x;
            float current_y = current_pose.pose.position.y;

            // 计算从杆中心到无人机当前位置的角度作为起始角度
            start_angle = atan2(current_y - pole_y, current_x - pole_x);

            // 初始化偏航角为当前偏航角，避免突然变化
            current_yaw = tf::getYaw(current_pose.pose.orientation);
            yaw_initialized = true;

            // 检查无人机是否已经在合理的绕杆半径范围内
            float current_distance = sqrt(pow(current_x - pole_x, 2) + pow(current_y - pole_y, 2));

            // 如果距离差异较大，先移动到合适的半径位置
            if (fabs(current_distance - radius) > 0.3)
            {
                ROS_WARN("无人机距离杆子 %.2f 米，目标半径 %.2f 米，正在调整到合适位置",
                         current_distance, radius);

                // 计算目标位置（在当前角度上，但是正确的半径）
                float target_x = pole_x + radius * cos(start_angle);
                float target_y = pole_y + radius * sin(start_angle);

                geometry_msgs::PoseStamped adjust_target;
                adjust_target.header.frame_id = "map";
                adjust_target.header.stamp = ros::Time::now();
                adjust_target.pose.position.x = target_x;
                adjust_target.pose.position.y = target_y;
                adjust_target.pose.position.z = height;

                // 计算朝向杆子中心的偏航角
                float yaw = atan2(pole_y - target_y, pole_x - target_x);
                tf::Quaternion q_tf = tf::createQuaternionFromYaw(yaw);
                geometry_msgs::Quaternion target_q;
                tf::quaternionTFToMsg(q_tf, target_q);
                adjust_target.pose.orientation = target_q;

                local_pose_pub.publish(adjust_target);

                // 检查是否到达调整位置
                float adjust_dist = sqrt(pow(target_x - current_x, 2) + pow(target_y - current_y, 2));
                if (adjust_dist > 0.2)
                {
                    return 0; // 还在调整位置，未开始绕杆
                }
            }

            // 设置结束角度为完整一圈后回到起始位置
            end_angle = start_angle + (clockwise ? -2 * M_PI : 2 * M_PI);

            // 计算需要转过的总角度（固定为一整圈）
            total_angle_to_travel = 2 * M_PI;

            angle_traveled = 0;          // 初始已转过的角度为0
            current_angle = start_angle; // 设置当前角度为起始角度
            raw_angle = start_angle;     // 初始化原始角度
            prev_raw_angle = raw_angle;  // 初始化前一个原始角度
            last_time = ros::Time::now();

            ROS_INFO("绕杆飞行初始化: 从无人机当前位置开始，起始角度 %.2f 度，方向: %s，总计角度: %.2f",
                     start_angle * 180.0 / M_PI, clockwise ? "顺时针" : "逆时针", total_angle_to_travel);

            initialized = true;
        }

        // 计算时间差
        ros::Time current_time = ros::Time::now();
        double dt = (current_time - last_time).toSec();
        last_time = current_time;

        // 防止dt太大
        if (dt > 0.1)
            dt = 0.1;

        // 计算这一步要转过的角度
        float step_angle = dt * angular_speed;

        // 保存前一个原始角度
        prev_raw_angle = raw_angle;

        // 根据旋转方向更新原始角度
        if (clockwise)
        {
            raw_angle -= step_angle; // 顺时针减小
        }
        else
        {
            raw_angle += step_angle; // 逆时针增加
        }

        // 更新标准化后的当前角度（用于计算位置）
        current_angle = atan2(sin(raw_angle), cos(raw_angle));

        // 更新已经转过的总角度（直接累加step_angle，不考虑标准化）
        angle_traveled += step_angle;

        // 检查是否完成绕杆
        if (angle_traveled >= total_angle_to_travel)
        {
            ROS_INFO("绕杆飞行完成: 已转过 %.2f，目标 %.2f", angle_traveled, total_angle_to_travel);
            initialized = false; // 重置状态
            angle_traveled = 0;
            return 1; // 返回完成状态
        }

        // 计算目标位置
        float px = radius * cos(current_angle) + pole_x;
        float py = radius * sin(current_angle) + pole_y;

        // 计算距离当前位置的距离
        float dist = sqrt(pow(px - current_pose.pose.position.x, 2) +
                          pow(py - current_pose.pose.position.y, 2));

        // 如果距离太大，调整角度
        if (dist > pose_limit)
        {
            float adjustment = step_angle * 0.7;

            // 调整原始角度和当前角度
            if (clockwise)
            {
                raw_angle += adjustment;
            }
            else
            {
                raw_angle -= adjustment;
            }

            current_angle = atan2(sin(raw_angle), cos(raw_angle));

            // 调整已转过的总角度
            angle_traveled -= adjustment;

            // 重新计算位置
            px = radius * cos(current_angle) + pole_x;
            py = radius * sin(current_angle) + pole_y;

            ROS_INFO("距离过远 (%.2f m)，减小角度步长", dist);
        }

        // 设置目标姿态
        geometry_msgs::PoseStamped target_pose;
        target_pose.header.frame_id = "map";
        target_pose.header.stamp = current_time;
        target_pose.pose.position.x = px;
        target_pose.pose.position.y = py;
        target_pose.pose.position.z = height;

        // 平滑偏航角过渡 - 避免突然的姿态变化
        float target_yaw = atan2(pole_y - py, pole_x - px); // 理想的朝向杆子中心的偏航角

        // 计算偏航角误差并进行角度归一化
        float yaw_error = target_yaw - current_yaw;
        while (yaw_error > M_PI)
            yaw_error -= 2 * M_PI;
        while (yaw_error < -M_PI)
            yaw_error += 2 * M_PI;

        // 限制偏航角变化速率，最大30度/秒 = 0.523 rad/s
        float max_yaw_rate = 0.523; // 30度/秒
        float max_yaw_step = max_yaw_rate * dt;

        // 平滑调整偏航角
        if (fabs(yaw_error) > max_yaw_step)
        {
            current_yaw += (yaw_error > 0) ? max_yaw_step : -max_yaw_step;
        }
        else
        {
            current_yaw = target_yaw; // 已经接近目标，直接设置
        }

        // 标准化当前偏航角
        current_yaw = atan2(sin(current_yaw), cos(current_yaw));

        tf::Quaternion q_tf = tf::createQuaternionFromYaw(current_yaw);
        geometry_msgs::Quaternion target_q;
        tf::quaternionTFToMsg(q_tf, target_q);
        target_pose.pose.orientation = target_q;

        // 发布目标位姿
        local_pose_pub.publish(target_pose);

        // 计算完成百分比
        float completion_percent = 100.0 * angle_traveled / total_angle_to_travel;

        // 输出调试信息
        ROS_INFO_THROTTLE(1.0, "绕杆进行中: 当前角度 %.2f, 原始角度 %.2f, 目标角度 %.2f, 已转过 %.2f/%.2f (%.1f%%), 偏航角 %.1f°→%.1f°",
                          current_angle, raw_angle, end_angle, angle_traveled, total_angle_to_travel,
                          completion_percent, current_yaw * 180.0 / M_PI, target_yaw * 180.0 / M_PI);

        return 0; // 未完成
    }

    /**
     * 执行旋转动作 - 缓慢旋转且固定位置
     * @param targetYaw 目标偏航角（弧度）
     * @param rate 控制频率
     * @return true=完成，false=未完成
     */
    bool performRotation(double targetYaw, ros::Rate &rate)
    {
        static double lastTargetYaw = -999.0; // 使用特殊值标识未初始化
        static double currentTargetYaw = 0.0;
        static geometry_msgs::PoseStamped fixedPosition; // 固定旋转位置
        static bool rotationInitialized = false;

        double currentYaw = tf::getYaw(current_pose.pose.orientation);
        double yawError = targetYaw - currentYaw;

        // 角度归一化到[-π, π]
        while (yawError > M_PI)
            yawError -= 2 * M_PI;
        while (yawError < -M_PI)
            yawError += 2 * M_PI;

        // 检查是否需要重置旋转状态
        extern bool rotation_reset_flag;
        if (rotation_reset_flag)
        {
            rotationInitialized = false;
            rotation_reset_flag = false;
        }

        // 初始化旋转 - 改进版本
        if (!rotationInitialized || fabs(targetYaw - lastTargetYaw) > 0.01)
        {
            // 锁定当前位置作为旋转中心
            fixedPosition = current_pose;
            fixedPosition.header.stamp = ros::Time::now();
            fixedPosition.header.frame_id = "map";
            currentTargetYaw = currentYaw;
            rotationInitialized = true;
            lastTargetYaw = targetYaw;

            ROS_INFO("Rotation position locked at [%.3f, %.3f, %.3f]",
                     fixedPosition.pose.position.x, fixedPosition.pose.position.y, fixedPosition.pose.position.z);
            ROS_INFO("Starting rotation from %.2f to %.2f degrees",
                     currentYaw * 180.0 / M_PI, targetYaw * 180.0 / M_PI);
        }

        // 计算旋转步长：增大到30度/秒，减少调整频率
        double dt = rate.expectedCycleTime().toSec();
        double maxRotationStep = 0.523 * dt; // 30度/秒 = 0.523弧度/秒

        // 计算到目标角度的误差
        double remainingError = targetYaw - currentTargetYaw;
        while (remainingError > M_PI)
            remainingError -= 2 * M_PI;
        while (remainingError < -M_PI)
            remainingError += 2 * M_PI;

        // 限制每个控制周期的旋转量
        if (fabs(remainingError) > maxRotationStep)
        {
            currentTargetYaw += (remainingError > 0) ? maxRotationStep : -maxRotationStep;
        }
        else
        {
            currentTargetYaw = targetYaw; // 已经接近目标，直接设置
        }

        // 使用位置控制进行旋转 - 严格固定位置版本
        geometry_msgs::PoseStamped rotation_pose;
        rotation_pose.header.stamp = ros::Time::now();
        rotation_pose.header.frame_id = "map";

        // 严格使用锁定的位置，完全不使用当前位置
        rotation_pose.pose.position.x = fixedPosition.pose.position.x;
        rotation_pose.pose.position.y = fixedPosition.pose.position.y;
        rotation_pose.pose.position.z = fixedPosition.pose.position.z;

        // 设置当前步骤的目标偏航角
        tf::Quaternion q_tf = tf::createQuaternionFromYaw(currentTargetYaw);
        geometry_msgs::Quaternion target_q;
        tf::quaternionTFToMsg(q_tf, target_q);
        rotation_pose.pose.orientation = target_q;

        // 发布位置命令
        local_pose_pub.publish(rotation_pose);

        // 检查是否到达目标 - 增大容忍度
        if (fabs(yawError) < 0.047)
        {                                // 2.5度误差内认为到达 (0.047弧度≈2.5度)
            rotationInitialized = false; // 重置状态以备下次使用
            lastTargetYaw = -999.0;      // 重置标识
            ROS_INFO("Rotation completed! Final yaw: %.2f degrees at fixed position [%.3f, %.3f, %.3f]",
                     currentYaw * 180.0 / M_PI,
                     fixedPosition.pose.position.x, fixedPosition.pose.position.y, fixedPosition.pose.position.z);
            return true;
        }

        // 每2秒输出一次调试信息
        static ros::Time last_debug_time;
        static bool debug_initialized = false;
        if (!debug_initialized)
        {
            last_debug_time = ros::Time::now();
            debug_initialized = true;
        }

        if ((ros::Time::now() - last_debug_time).toSec() >= 2.0)
        {
            ROS_INFO("Rotating: current=%.1f°, target=%.1f°, error=%.1f°",
                     currentYaw * 180.0 / M_PI, targetYaw * 180.0 / M_PI, yawError * 180.0 / M_PI);
            ROS_INFO("Fixed position: [%.3f, %.3f, %.3f], Current position: [%.3f, %.3f, %.3f]",
                     fixedPosition.pose.position.x, fixedPosition.pose.position.y, fixedPosition.pose.position.z,
                     current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
            last_debug_time = ros::Time::now();
        }

        return false;
    }

    /**
     * 执行圆形平移飞行
     * @param centerX 圆心X坐标
     * @param centerY 圆心Y坐标
     * @param centerZ 圆心Z坐标
     * @param radius 半径
     * @param elapsedTime 已经过的时间（秒）
     * @param rate 控制频率
     * @return 当前位置到目标位置的距离，用于判断是否到达
     */
    int performCircleFlight(double centerX, double centerY, double centerZ, double radius, double elapsedTime, ros::Rate &rate)
    {
        double angularSpeed = 0.5; // 角速度 rad/s
        double angle = angularSpeed * elapsedTime;

        double targetX = centerX + radius * cos(angle);
        double targetY = centerY + radius * sin(angle);

        geometry_msgs::PoseStamped target;
        target.pose.position.x = targetX;
        target.pose.position.y = targetY;
        target.pose.position.z = centerZ;

        return fly_to_target(target, local_pose_pub, rate);
    }

    /**
     * 执行安全降落 - 位置控制到15cm后AUTO.LAND
     * @param descent_rate 下降速率（m/s）
     * @param rate 控制频率
     * @return true=完成，false=未完成
     */
    bool performSafeLanding(double descent_rate, ros::Rate &rate)
    {
        static double landing_target_height = 0.0;
        static bool landing_initialized = false;
        static bool auto_land_triggered = false;

        if (!landing_initialized)
        {
            landing_target_height = current_pose.pose.position.z;
            landing_initialized = true;
            auto_land_triggered = false;
            ROS_WARN("Starting safe landing from %.2f meters", landing_target_height);
        }

        // 第一阶段：使用fly_to_target函数缓慢降落到20cm
        if (!auto_land_triggered)
        {
            // 缓慢下降
            double dt = rate.expectedCycleTime().toSec();
            landing_target_height -= descent_rate * dt;

            // 限制最低高度为20cm
            if (landing_target_height < 0.13)
            {
                landing_target_height = 0.10;
            }

            // 使用fly_to_target函数进行精确降落控制
            geometry_msgs::PoseStamped landing_target;
            landing_target.header.stamp = ros::Time::now();
            landing_target.header.frame_id = "map";
            landing_target.pose.position.x = current_pose.pose.position.x;
            landing_target.pose.position.y = current_pose.pose.position.y;
            landing_target.pose.position.z = landing_target_height;
            landing_target.pose.orientation = current_pose.pose.orientation;

            // 使用fly_to_target函数实现平滑降落
            int landing_result = fly_to_target(landing_target, local_pose_pub, rate);

            // 输出降落调试信息
            static ros::Time last_landing_debug_time;
            static bool landing_debug_initialized = false;
            if (!landing_debug_initialized)
            {
                last_landing_debug_time = ros::Time::now();
                landing_debug_initialized = true;
            }

            if ((ros::Time::now() - last_landing_debug_time).toSec() >= 2.0)
            {
                ROS_INFO("Landing: Current height: %.2fm, Target height: %.2fm, Speed: %.2fm/s",
                         current_pose.pose.position.z, landing_target_height, descent_rate);
                last_landing_debug_time = ros::Time::now();
            }

            // 当接近13cm时，切换到AUTO.LAND
            if (current_pose.pose.position.z <= 0.13)
            {
                auto_land_triggered = true;
                landing_debug_initialized = false; // 重置调试状态
                ROS_WARN("Switching to AUTO.LAND at %.2f meters", current_pose.pose.position.z);
            }

            return false;
        }

        // 第二阶段：AUTO.LAND最终降落
        static ros::Time land_request_time = ros::Time::now();
        mavros_msgs::SetMode land_mode;
        land_mode.request.custom_mode = "AUTO.LAND";

        if (current_state.mode != "AUTO.LAND" &&
            (ros::Time::now() - land_request_time > ros::Duration(2.0)))
        {
            if (set_mode_client.call(land_mode) && land_mode.response.mode_sent)
            {
                ROS_INFO("AUTO.LAND enabled for final touchdown");
                // 重置状态以备下次使用
                landing_initialized = false;
                auto_land_triggered = false;
                return true; // 降落完成
            }
            land_request_time = ros::Time::now();
        }

        return false;
    }
};

// 动作类型枚举
enum ActionType
{
    MOVE_TO_POINT, // 移动到指定点
    ROTATE,        // 旋转到指定角度
    TOPIC_CONTROL, // 接收话题控制
    CIRCLE,        // 圆形平移飞行
    POLE_SURROUND, // 绕杆飞行
    HOVER,         // 悬停
    WAIT,          // 等待指定时间
    SAFE_LANDING   // 安全降落（位置控制到30cm后自动降落）
};

// 增强版航路点结构体
struct Waypoint
{
    ActionType actionType;
    double x, y, z;        // 位置坐标
    double yaw;            // 目标偏航角
    double duration;       // 持续时间(秒)
    double speed;          // 移动速度
    std::string stepName;  // 步骤名称
    std::string topicName; // 话题名称(用于TOPIC_CONTROL)
    double radius;         // 圆形飞行半径

    // 统一的构造函数
    Waypoint(ActionType type, double x_val = 0, double y_val = 0, double z_val = 0,
             double yaw_val = 0, double duration_val = 0, double speed_val = 0.5,
             const std::string &name = "", const std::string &topic = "", double radius_val = 0)
        : actionType(type), x(x_val), y(y_val), z(z_val), yaw(yaw_val),
          duration(duration_val), speed(speed_val), stepName(name), topicName(topic), radius(radius_val) {}
};

// 增强版航路点管理类
class WaypointManager
{
private:
    std::list<Waypoint> waypoints;
    std::list<Waypoint>::iterator currentWaypoint;
    bool initialized;
    ros::Time actionStartTime;
    bool actionInProgress;
    double initialYaw;
    geometry_msgs::PoseStamped topicControlMsg;
    ros::Subscriber topicControlSub;
    bool hasTopicControl;
    std::string currentTopicName;  // 当前话题名称
    ros::NodeHandle* nh_ptr;       // NodeHandle指针

public:
    WaypointManager() : initialized(false), actionInProgress(false), hasTopicControl(false), nh_ptr(nullptr)
    {
    }

    // 设置NodeHandle指针
    void setNodeHandle(ros::NodeHandle* nh) 
    {
        nh_ptr = nh;
    }

    // 话题控制回调函数
    void topicControlCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
    {
        topicControlMsg = *msg;
        hasTopicControl = true;
        ROS_INFO_THROTTLE(2.0, "Received topic control message from %s: [%.2f, %.2f, %.2f]", 
                         currentTopicName.c_str(),
                         msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    }

    // 重置话题控制状态
    void resetTopicControl()
    {
        hasTopicControl = false;
        
        // 关闭当前订阅器
        if (topicControlSub)
        {
            topicControlSub.shutdown();
            ROS_INFO("Topic control subscriber shutdown for: %s", currentTopicName.c_str());
        }
        
        // 清空消息和话题名称
        topicControlMsg = geometry_msgs::PoseStamped();
        currentTopicName.clear();
        
        // 重置速度控制状态，防止瞬移
        velocity_reset_flag = true;
        
        ROS_INFO("Topic control state reset - all messages cleared and velocity control reset");
    }

    // 设置话题订阅
    void setupTopicSubscription(ros::NodeHandle &nh, const std::string &topicName)
    {
        // 如果是同一个话题，不需要重新订阅
        if (currentTopicName == topicName && topicControlSub)
        {
            ROS_INFO("Already subscribed to topic: %s", topicName.c_str());
            return;
        }

        // 重置话题控制状态（这会自动设置velocity_reset_flag）
        resetTopicControl();
        
        // 额外确保速度控制重置
        velocity_reset_flag = true;
        ROS_WARN("Topic switching from '%s' to '%s' - forcing velocity control reset", 
                 currentTopicName.c_str(), topicName.c_str());

        // 订阅新话题
        currentTopicName = topicName;
        topicControlSub = nh.subscribe<geometry_msgs::PoseStamped>(
            topicName, 10, &WaypointManager::topicControlCallback, this);
        
        ROS_INFO("Subscribed to new topic: %s", topicName.c_str());
    }

    bool hasNextWaypoint()
    {
        return initialized && (currentWaypoint != waypoints.end());
    }

    Waypoint getCurrentWaypoint()
    {
        if (currentWaypoint != waypoints.end())
        {
            return *currentWaypoint;
        }
        return Waypoint(MOVE_TO_POINT, 0, 0, 0, 0, 0, 0, "invalid");
    }

    void moveToNextWaypoint()
    {
        if (currentWaypoint != waypoints.end())
        {
            // 如果当前是话题控制，在移动到下一个航路点前重置状态
            Waypoint current = getCurrentWaypoint();
            if (current.actionType == TOPIC_CONTROL)
            {
                resetTopicControl();
                ROS_INFO("Topic control ended, moving to next waypoint");
            }
            
            ++currentWaypoint;
            actionInProgress = false; // 重置动作状态
            
            // 无论切换到什么类型的航路点，都重置速度控制状态
            // 这样可以确保新的任务从当前位置开始，避免瞬移
            velocity_reset_flag = true;
            ROS_INFO("Moving to next waypoint - velocity control will be reset");
        }
    }

    // 开始当前动作
    void startCurrentAction()
    {
        actionStartTime = ros::Time::now();
        actionInProgress = true;

        // 为不同动作类型进行特殊初始化
        Waypoint current = getCurrentWaypoint();
        if (current.actionType == ROTATE)
        {
            initialYaw = tf::getYaw(current_pose.pose.orientation);
            // 重置旋转状态，确保每次旋转都锁定新的位置
            resetRotationPosition();
        }
        else if (current.actionType == HOVER || current.actionType == WAIT)
        {
            // 重置悬停状态，确保每次悬停都锁定新的位置
            resetHoldPosition();
        }
        else if (current.actionType == TOPIC_CONTROL)
        {
            // 为话题控制设置订阅
            if (nh_ptr)
            {
                // 在开始话题控制前强制重置所有相关状态
                velocity_reset_flag = true;
                resetHoldPosition();  // 重置悬停位置，防止回到旧位置
                
                ROS_WARN("Starting topic control - forcing velocity and hold reset to prevent teleportation");
                
                setupTopicSubscription(*nh_ptr, current.topicName);
                ROS_INFO("Starting topic control for: %s (duration: %.1fs)", 
                         current.topicName.c_str(), current.duration);
            }
            else
            {
                ROS_ERROR("NodeHandle not set! Cannot subscribe to topic control.");
            }
        }
    }

    // 重置悬停位置锁定状态
    void resetHoldPosition()
    {
        // 由于C++静态变量的限制，我们通过一个特殊的方法来重置
        // 这个方法会在下一次调用hold()时强制重新初始化位置
        extern void reset_hold_state(); // 声明外部函数
        reset_hold_state();
    }

    // 重置旋转位置锁定状态
    void resetRotationPosition()
    {
        // 由于C++静态变量的限制，我们通过一个特殊的方法来重置
        // 这个方法会在下一次调用performRotation()时强制重新初始化位置
        extern void reset_rotation_state(); // 声明外部函数
        reset_rotation_state();
    }

    // 便捷的航路点添加方法
    void addMovePoint(double x, double y, double z, const std::string &name)
    {
        waypoints.emplace_back(MOVE_TO_POINT, x, y, z, 0, 0, 0.5, name);
    }

    void addRotation(double yaw, const std::string &name)
    {
        waypoints.emplace_back(ROTATE, 0, 0, 0, yaw, 0, 0, name);
    }

    void addTopicControl(const std::string &topicName, double duration, const std::string &name)
    {
        waypoints.emplace_back(TOPIC_CONTROL, 0, 0, 0, 0, duration, 0, name, topicName);
    }

    void addCircleFlight(double centerX, double centerY, double centerZ,
                         double radius, double duration, const std::string &name)
    {
        waypoints.emplace_back(CIRCLE, centerX, centerY, centerZ, 0, duration, 0.5, name, "", radius);
    }

    void addPoleSurround(double pole_x, double pole_y, double radius, double height, const std::string &name)
    {
        waypoints.emplace_back(POLE_SURROUND, pole_x, pole_y, height, 0, 0, 0.5, name, "", radius);
    }

    void addHover(double duration, const std::string &name)
    {
        waypoints.emplace_back(HOVER, 0, 0, 0, 0, duration, 0, name);
    }

    void addWait(double duration, const std::string &name)
    {
        waypoints.emplace_back(WAIT, 0, 0, 0, 0, duration, 0, name);
    }

    void addSafeLanding(const std::string &name)
    {
        waypoints.emplace_back(SAFE_LANDING, 0, 0, 0, 0, 0, 0.2, name); // speed用作下降速率
    }

    void reset()
    {
        waypoints.clear();
        actionInProgress = false;
        initialized = false;
    }

    void finalize()
    {
        if (!waypoints.empty())
        {
            currentWaypoint = waypoints.begin();
            initialized = true;
            ROS_INFO("WaypointManager initialized with %lu waypoints", waypoints.size());

            // 打印所有航路点信息
            int index = 0;
            for (auto it = waypoints.begin(); it != waypoints.end(); ++it)
            {
                ROS_INFO("Waypoint %d: %s (Type: %d)", index++, it->stepName.c_str(), it->actionType);
            }
        }
        else
        {
            ROS_WARN("No waypoints to initialize!");
        }
    }

    // 检查当前动作是否完成
    bool isCurrentActionComplete(move_map &move1, ros::Publisher &local_vel_pub, ros::Rate &rate)
    {
        if (!actionInProgress)
        {
            startCurrentAction();
            return false;
        }

        Waypoint current = getCurrentWaypoint();
        ros::Duration elapsed = ros::Time::now() - actionStartTime;

        switch (current.actionType)
        {
        case MOVE_TO_POINT:
        {
            geometry_msgs::PoseStamped target;
            target.pose.position.x = current.x;
            target.pose.position.y = current.y;
            target.pose.position.z = current.z;
            return move1.fly_to_target(target, local_pose_pub, rate) == 1;
        }

        case ROTATE:
        {
            return move1.performRotation(current.yaw, rate);
        }

        case TOPIC_CONTROL:
        {
            if (elapsed.toSec() >= current.duration)
            {
                ROS_INFO("Topic control duration reached (%.1fs), ending control", current.duration);
                return true; // 时间到了，结束话题控制
            }
            
            // 检查当前话题是否与上次不同，如果不同则重新订阅
            if (currentTopicName != current.topicName)
            {
                ROS_WARN("Topic changed from '%s' to '%s', re-subscribing", 
                         currentTopicName.c_str(), current.topicName.c_str());
                resetTopicControl();
                if (nh_ptr)
                {
                    setupTopicSubscription(*nh_ptr, current.topicName);
                }
            }
            
            if (!hasTopicControl)
            {
                // 关键修复：当没有话题控制消息时，不要使用旧的悬停位置
                // 而是直接使用速度控制保持当前位置，避免飞向旧位置
                static ros::Time last_no_control_time;
                static bool no_control_initialized = false;
                
                if (!no_control_initialized) {
                    last_no_control_time = ros::Time::now();
                    no_control_initialized = true;
                }
                
                // 使用零速度的速度控制来保持位置，而不是使用hold函数
                move1.pose_velocity_ctrl(0.0, 0.0, 0.0, 0.0, rate);
                
                ROS_WARN_THROTTLE(5.0, "No topic control message received from %s, using velocity control to hold current position.", 
                                 current.topicName.c_str());
            }
            else
            {
                double vx = std::max(-1.0, std::min(1.0, topicControlMsg.pose.position.x));
                double vy = std::max(-1.0, std::min(1.0, topicControlMsg.pose.position.y));
                double vz = 0;
                double yaw = 0;
                // double yaw = std::max(-1.0, std::min(1.0, topicControlMsg.pose.orientation.w)) * 0.5;
                // 使用接收到的话题控制消息
                move1.pose_velocity_ctrl(vx, vy, vz, yaw, rate);
                
                ROS_INFO_THROTTLE(2.0, "Topic control active from %s: velocity [%.2f, %.2f, %.2f]",
                                 current.topicName.c_str(), vx, vy, vz);
            }
            return false;
        }

        case CIRCLE:
        {
            if (elapsed.toSec() >= current.duration)
            {
                return true;
            }
            return move1.performCircleFlight(current.x, current.y, current.z, current.radius, elapsed.toSec(), rate) == 1;
        }

        case POLE_SURROUND:
        {
            // 使用 performPoleSurrounding 函数执行绕杆飞行
            return move1.performPoleSurrounding(current.x, current.y, current.radius, current.z) == 1;
        }

        case HOVER:
        case WAIT:
        {
            move1.hold(local_pose_pub); // 使用位置发布器
            return elapsed.toSec() >= current.duration;
        }

        case SAFE_LANDING:
        {
            return move1.performSafeLanding(current.speed, rate);
        }

        default:
            return true;
        }
    }

    // 析构函数，确保资源清理
    ~WaypointManager()
    {
        resetTopicControl();
    }

private:
};

// 示例：绕杆测试任务
void createPoleSurroundTestMission(WaypointManager &manager)
{
    ROS_WARN("Creating Pole Surround Test Mission...");
    manager.reset();

    // 第一个任务：起飞到1.5米高度
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addMovePoint(3.0, 2.0, 1.5, "takeoff_to_height");

    // 移动到杆子位置并绕杆
    manager.addPoleSurround(3.0, 3.0, 1.0, 1.5, "performPoleSurrounding");
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addRotation(0.0, "scan_front");
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");

    // 添加安全降落作为最后一个动作
    manager.addSafeLanding("safe_landing");

    manager.finalize();
    ROS_WARN("Pole Surround Test Mission Created successfully!");
}

// 示例：包含绕杆的巡逻任务
void createPatrolMissionWithPole(WaypointManager &manager)
{
    ROS_WARN("Creating Patrol Mission with Pole Surround...");
    manager.reset();
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addMovePoint(3.0, 0.0, 1.5, "patrol_point_1");
    manager.addRotation(M_PI / 2, "scan_right");
    manager.addHover(3.0, "observe_1");
    manager.addMovePoint(3.0, 3.0, 1.5, "patrol_point_2");
    manager.addRotation(M_PI, "scan_back");
    manager.addHover(3.0, "observe_2");
    manager.addMovePoint(0.0, 3.0, 1.5, "patrol_point_3");
    manager.addRotation(-M_PI / 2, "scan_left");
    manager.addHover(3.0, "observe_3");
    // 绕杆飞行
    manager.addPoleSurround(1.5, 1.5, 1.0, 1.5, "performPoleSurrounding");
    // manager.addTopicControl("/vision/target", 5.0, "vision_target_control");
    manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_4");
    manager.addRotation(0.0, "scan_front");
    manager.addHover(3.0, "observe_4");
    manager.addSafeLanding("safe_landing");
    manager.finalize();
    ROS_WARN("Patrol Mission with Pole Surround Created successfully!");
}

// 示例：包含话题的巡逻任务
void createPatrolMissionWithTopic(WaypointManager &manager)
{
    ROS_WARN("Creating Patrol Mission with Topic Control...");
    manager.reset();
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addMovePoint(3.0, 0.0, 1.5, "patrol_point_1");
    // manager.addRotation(M_PI / 2, "scan_right");
    // manager.addHover(3.0, "observe_1");
    // manager.addMovePoint(3.0, 3.0, 1.5, "patrol_point_2");
    // manager.addRotation(M_PI, "scan_back");
    // manager.addHover(3.0, "observe_2");
    // manager.addMovePoint(0.0, 3.0, 1.5, "patrol_point_3");
    // manager.addRotation(-M_PI / 2, "scan_left");
    // manager.addHover(3.0, "observe_3");
    // 绕杆飞行
    // manager.addPoleSurround(1.5, 1.5, 1.0, 1.5, "performPoleSurrounding");
    manager.addTopicControl("/vision_target", 15.0, "vision_target_control");
    manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_4");
    manager.addTopicControl("/vision_murmur", 15.0, "vision_target_control");
    manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_4");
    manager.addMovePoint(2.2, 2.2, 1.5, "patrol_point_5");
    manager.addTopicControl("/vision_mur", 15.0, "vision_target_control");
    manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_4");
    manager.addRotation(0.0, "scan_front");
    manager.addHover(3.0, "observe_4");
    manager.addSafeLanding("safe_landing");
    manager.finalize();
    ROS_WARN("Patrol Mission with Pole Surround Created successfully!");
}

int main(int argc, char **argv)
{
    double target_speed = 0.5;
    int rotation_direction = 0;
    std_msgs::ColorRGBA lidar_data_sender;
    double *pole_data;
    double yaw_angle, start_yaw;
    std_msgs::Int8 vision_cmd_sender;
    std_msgs::Int16 led_data_sender;
    led_data_sender.data = 0;
    vision_cmd_sender.data = 1;

    // 创建航路点管理器并设置初始任务
    WaypointManager waypointManager;
    
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    
    // 设置NodeHandle到WaypointManager中
    waypointManager.setNodeHandle(&nh);
    
    // 可以使用预定义的任务
    // createPatrolMissionWithPole(waypointManager); // 包含绕杆的巡逻任务
    createPatrolMissionWithTopic(waypointManager); // 包含话题控制的巡逻任务
    // createPoleSurroundTestMission(waypointManager); // 绕杆测试任务
    // 订阅命令消息

    // 订阅飞行器状态和发布位置设定点
    state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 10, state_cb);                     // 订阅状态
    pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, pose_cb); // 订阅位置信息.
    vel_sub = nh.subscribe<geometry_msgs::TwistStamped>("/mavros/local_position/velocity", 10, vel_cb);
    local_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 10);   // 设置位置
    local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10); // 设定速度
    set_raw_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);          // 以自身为坐标系的速度控制
    state_message_pub = nh.advertise<std_msgs::Int8>("/offboard_node/state", 10);
    // 定义服务客户端，用于解锁/上锁无人机和切换到离线控制模式
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
    ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land");
    ros::ServiceServer state_client = nh.advertiseService("/command", state_cb);

    // 指定发布位置设定点的频率
    ros::Rate rate(50.0);

    // 等待 FCU 连接
    while (ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    // 发布一些起始位置设定点，然后才开始执行控制指令
    geometry_msgs::TwistStamped vector;
    // send a few setpoints before starting
    for (int i = 100; ros::ok() && i > 0; --i)
    {
        local_vel_pub.publish(vector);
        ros::spinOnce();
        rate.sleep();
    }

    // 请求进入离线控制模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // 请求解锁飞行器
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 记录上一次请求时间
    ros::Time last_request = ros::Time::now();
    ros::Time last_time = ros::Time::now();
    ros::Time fire_time = ros::Time::now();
    mavros_msgs::CommandTOL land_cmd;
    std_msgs::Int8 offboard_state;
    std_msgs::Int8 drop_command;
    drop_command.data = 0;
    geometry_msgs::PoseStamped target_position;
    geometry_msgs::PoseStamped landing_position;
    geometry_msgs::PoseStamped rounding_pose[3];
    land_cmd.request.altitude = 0.0;
    land_cmd.request.yaw = 0.0;

    // 用于步骤
    int flag = 0;

    // 定义class的实例
    move_map move1;

    while (ros::ok())
    {
        ros::spinOnce();
        // ROS_INFO("Pose-z: %f",current_pose.pose.position.z);
        // 无人机状态设定与判断
        // 进入while循环后，先循环5s，然后再向客户端发送无人机状态设置的消息
        // set_mode_client.call   arming_client.call
        if (current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                ROS_WARN("Offboard enabled");
            }
            last_request = ros::Time::now();
        }
        else
        {
            if (!current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                if (arming_client.call(arm_cmd) &&
                    arm_cmd.response.success)
                {
                    ROS_WARN("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
            else //  无人机 Offboard enabled && Vehicle armed 后
            {
                // 使用增强版链表优化的航路点控制 - 统一任务管理
                static bool mission_initialized = false;

                // 一次性初始化任务系统
                if (!mission_initialized)
                {
                    flag = 0;
                    last_time = ros::Time::now();
                    mission_initialized = true;

                    // 开始第一个航路点任务
                    if (waypointManager.hasNextWaypoint())
                    {
                        Waypoint currentWP = waypointManager.getCurrentWaypoint();
                        ROS_WARN("Mission started! First task: %s (Action: %d)", currentWP.stepName.c_str(), currentWP.actionType);
                    }
                }

                // 执行航路点导航
                if (waypointManager.hasNextWaypoint())
                {
                    Waypoint currentWP = waypointManager.getCurrentWaypoint();

                    // 为话题控制设置订阅
                    if (currentWP.actionType == TOPIC_CONTROL)
                    {
                        waypointManager.setupTopicSubscription(nh, currentWP.topicName);
                    }

                    bool actionComplete = waypointManager.isCurrentActionComplete(move1, local_vel_pub, rate);

                    if (actionComplete)
                    {
                        last_time = ros::Time::now();
                        ROS_INFO("Completed %s", currentWP.stepName.c_str());

                        // 移动到下一个航路点
                        waypointManager.moveToNextWaypoint();

                        if (waypointManager.hasNextWaypoint())
                        {
                            Waypoint nextWP = waypointManager.getCurrentWaypoint();
                            ROS_INFO("Starting %s (Action: %d)", nextWP.stepName.c_str(), nextWP.actionType);
                            ROS_WARN("Starting %s (Action: %d)", nextWP.stepName.c_str(), nextWP.actionType);
                        }
                        else
                        {
                            // 所有航路点完成，任务结束
                            ROS_INFO("All waypoints completed, mission finished");
                            ROS_WARN("All waypoints completed, mission finished");
                        }
                    }
                }
                else
                {
                    ROS_WARN("No more waypoints, mission finished");
                }
            }
        }
        rate.sleep();
    }
    return 0;
}
